package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class EnteringMazeDirectSequence extends RobotRunningSequence {
	/**
	 * Alignment threshold beyond which we should adjust the orientation of the robot.
	 */
	private final static int ALIGNMENT_THRESHOLD = 5;	// distance from center
	

	public enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Entering maze direct", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				machine.changeState(AdjustOrientation);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		AdjustOrientation {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				IDetector detector = robot.getMazeEntranceGuideDetector();
				if (detector.isPresent()) {
					int d = RobotRunningSequence.getOrientation(machine.robot(), detector);
					
					String log = String.format("Turning degree: %d", d);
					robot.log(log, 4);

					if (Math.abs(d) > ALIGNMENT_THRESHOLD) {
						robot.sendCommand(RobotAction.TurnDegree, d);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					} 
					
					machine.changeState(Move);
				}
				else {
					machine.changeState(EntranceNotFound);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Move {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Done {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		EntranceNotFound {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};
		
		static EnteringMazeDirectSequence m_sequence;

		public static void setSequence(EnteringMazeDirectSequence sequence) {
			m_sequence = sequence;
		}

	}
	
	public EnteringMazeDirectSequence(RobotRunningStateMachine running) {
		super(running, State.Start);

		State.setSequence(this);
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(false);
		robot.enableMaze2Detector(false);
		robot.enableMaze3Detector(false);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(false);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.Done);
	}

	@Override
	public boolean failed() {
		// TODO Auto-generated method stub
		return m_currState.equals(State.EntranceNotFound);
	}

}
